#!/usr/bin/python3

import roslib
import rospy
import math
import tf
import tf2_ros
from geometry_msgs.msg import Pose, Twist, TransformStamped, PointStamped, PoseStamped
import time
import numpy as np
import csv
from std_msgs.msg import Float64MultiArray
import copy
from p903_simulation.srv import ProductOnAGVControl, ProductOnAGVControlResponse
from tf2_geometry_msgs import tf2_geometry_msgs
from zhongduan.srv import GetKeyFeatureMeasure, GetKeyFeatureMeasureRequest
from pogo_agv.srv import GetPoseFromPoints, GetPoseFromPointsRequest, GetPoseFromPointsResponse
from gazebo_msgs.srv import SpawnModel, SpawnModelRequest, SpawnModelResponse
import threading
from pogo_agv.msg import MovePoseWithSupportPoints

class Product_on_agv_modulator():
    def __init__(self,agvName,productName,targetPose):
        self.agvName = agvName
        self.productName = productName
        self.targetPose = targetPose 
        self.srv_get_ball_position = rospy.ServiceProxy("/all_products/get_support_ball_positions", GetKeyFeatureMeasure)
        self.srv_get_support_pose = rospy.ServiceProxy("/"+ agvName + "/get_support_pose", GetPoseFromPoints)
        self.pub_move_pose = rospy.Publisher("/"+ agvName + "/move_to_support_points",MovePoseWithSupportPoints,queue_size=10)

        self.agvBase = self.agvName + "/base_link"
        self.productBase = self.productName + "/base_link"
        self.targetPoseName = self.productName + "/target_moving_pose"

        call_req = GetKeyFeatureMeasureRequest()
        call_req.key_feature_name.data = productName
        call_resp = self.srv_get_ball_position.call(call_req)

        self.product_support_points = call_resp.points

    def get_transfered_points(self,trans,point):
        ret = PointStamped()
        ret.header.frame_id = trans.frame_id
        ret.header.stamp = rospy.Time(0)

        return ret

    def run_control_step(self, tfBuffer, tfBroadcaster):
        tgt_trans = TransformStamped()
        tgt_trans.header.frame_id = self.targetPose.header.frame_id
        tgt_trans.header.stamp = rospy.Time.now()
        tgt_trans.child_frame_id = self.targetPoseName

        tgt_trans.transform.translation.x = self.targetPose.pose.position.x
        tgt_trans.transform.translation.y = self.targetPose.pose.position.y
        tgt_trans.transform.translation.z = self.targetPose.pose.position.z

        tgt_trans.transform.rotation.x = self.targetPose.pose.orientation.x
        tgt_trans.transform.rotation.y = self.targetPose.pose.orientation.y
        tgt_trans.transform.rotation.z = self.targetPose.pose.orientation.z
        tgt_trans.transform.rotation.w = self.targetPose.pose.orientation.w

        tfBroadcaster.sendTransform(tgt_trans)

        # call_req = GetKeyFeatureMeasureRequest()
        # call_req.key_feature_name.data = "agv_support"
        # call_resp = self.srv_get_ball_position.call(call_req)

        call_req = GetPoseFromPointsRequest()
        call_req.points = self.product_support_points
        call_resp = self.srv_get_support_pose.call(call_req)

        # product_trans = tfBuffer.lookup_transform(self.agvBase,self.productBase,rospy.Time(0))
        nominal_product_pose = call_resp.pose
        nominal_product_pose.header.stamp = rospy.Time(0)

        delta_pose = tfBuffer.transform(nominal_product_pose,self.productBase)
        delta_pose.header.frame_id = self.targetPoseName
        delta_pose.header.stamp = rospy.Time(0)
        nominal_control_pose = tfBuffer.transform(delta_pose,self.targetPose.header.frame_id)

        # nominal_control_points = [self.get_transfered_points(nominal_control_pose,i) for i in self.product_support_points]
        nominal_control_points = self.product_support_points

        print(self.targetPose)
        print(call_resp.pose)
        print(delta_pose)
        print(nominal_control_pose)

        sent_command = MovePoseWithSupportPoints()
        sent_command.pose = nominal_control_pose
        sent_command.points = nominal_control_points
        self.pub_move_pose.publish(sent_command)
        return

class p903_system_control():
    def __init__(self,nodename):
        rospy.init_node(nodename)
        self.srv_load_agv = rospy.Service("load_product_on_agv",ProductOnAGVControl, self.load_product_on_agv_cb)
        self.srv_replace_product = rospy.Service("replace_product",ProductOnAGVControl, self.replace_product_cb)
        self.srv_modulating_agv_product = rospy.Service("modulate_product_with_agv",ProductOnAGVControl, self.modulate_product_on_agv_cb)
        self.productOnAGVModulators = []

        self.tfBuffer = tf2_ros.Buffer()
        self.tfListener = tf2_ros.TransformListener(self.tfBuffer)        
        self.tfBroadcaster=tf2_ros.TransformBroadcaster()

        self.lock = threading.Lock()

    def modulate_product_on_agv_cb(self,req):
        print(req)
        agvName = req.agvName
        productName = req.productName
        targetPose = req.targetPose

        for i in range(len(self.productOnAGVModulators)):
            if(agvName == self.productOnAGVModulators[i].agvName and productName == self.productOnAGVModulators[i].productName):
                if(not req.isStartingControl):
                    self.lock.acquire()
                    self.productOnAGVModulators.pop(i)
                    self.lock.release()
                else:
                    self.productOnAGVModulators[i].targetPose = targetPose
                    
                return ProductOnAGVControlResponse(success = True)

        if(not rospy.has_param("/all_products/all_product_description")):
            return ProductOnAGVControlResponse(success = False)
        
        desc = rospy.get_param("/all_products/all_product_description")
        all_names = ["all_products/" + i['urdf_description']['prefix'] for i in desc]
        if(not req.productName in all_names):
            return ProductOnAGVControlResponse(success = False)

        modulator = Product_on_agv_modulator(agvName, productName, targetPose)
        time.sleep(1.0)
        modulator.run_control_step(self.tfBuffer,self.tfBroadcaster)

        # self.lock.acquire()
        # self.productOnAGVModulators.append(modulator)
        # self.lock.release()

        return ProductOnAGVControlResponse(success = True)

    def replace_product_cb(self,req):
        if(not rospy.has_param("/all_products/all_product_description")):
            return ProductOnAGVControlResponse(success = False)
        
        desc = rospy.get_param("/all_products/all_product_description")
        all_names = [i['urdf_description']['prefix'] for i in desc]
        if(not req.productName in all_names):
            return ProductOnAGVControlResponse(success = False)

        srv_load_product = rospy.ServiceProxy("/all_products/replace_gazebo_model", SpawnModel)

        call_req = SpawnModelRequest()
        call_req.model_name = req.agvName + ":" + req.productName
        call_req.reference_frame = req.agvName
        # call_req.initial_pose = call_resp.pose.pose
        call_resp = srv_load_product.call(call_req)

        return ProductOnAGVControlResponse(success = True)

    def load_product_on_agv_cb(self,req):

        if(not rospy.has_param("/all_products/all_product_description")):
            return ProductOnAGVControlResponse(success = False)
        
        desc = rospy.get_param("/all_products/all_product_description")
        all_names = [i['urdf_description']['prefix'] for i in desc]
        if(not req.productName in all_names):
            return ProductOnAGVControlResponse(success = False)

        srv_get_ball_position = rospy.ServiceProxy("/all_products/get_support_ball_positions", GetKeyFeatureMeasure)
        srv_get_support_pose = rospy.ServiceProxy("/"+ req.agvName + "/get_support_pose", GetPoseFromPoints)
        srv_load_product = rospy.ServiceProxy("/all_products/replace_gazebo_model", SpawnModel)

        call_req = GetKeyFeatureMeasureRequest()
        call_req.key_feature_name.data = "all_products/" + req.productName
        call_resp = srv_get_ball_position.call(call_req)

        call_req = GetPoseFromPointsRequest()
        call_req.points = call_resp.points
        call_resp = srv_get_support_pose.call(call_req)

        print(call_resp.pose)

        call_req = SpawnModelRequest()
        call_req.model_name = "all_products/" + req.productName + ":" + req.productName
        call_req.reference_frame = req.agvName
        call_req.initial_pose = call_resp.pose.pose
        # call_req.initial_pose.position.x = call_resp.pose.transform.translation.x
        # call_req.initial_pose.position.y = call_resp.pose.transform.translation.y
        # call_req.initial_pose.position.z = call_resp.pose.transform.translation.z

        # call_req.initial_pose.orientation.x = call_resp.pose.transform.rotation.x
        # call_req.initial_pose.orientation.y = call_resp.pose.transform.rotation.y
        # call_req.initial_pose.orientation.z = call_resp.pose.transform.rotation.z
        # call_req.initial_pose.orientation.w = call_resp.pose.transform.rotation.w

        call_resp = srv_load_product.call(call_req)

        return ProductOnAGVControlResponse(success = True)

    def run(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            # self.lock.acquire()
            # for i in range(len(self.productOnAGVModulators)):
            #     self.productOnAGVModulators[i].run_control_step(self.tfBuffer,self.tfBroadcaster)
            # self.lock.release()
            rate.sleep()

if __name__ == '__main__':
    service = p903_system_control('p903_system_control')
    service.run()
